#include "Converter.h"

#include "../../../RapidXML/rapidxml.hpp"
#include <fstream>

using std::cout;
using std::endl;

namespace MMM
{

Converter::Converter(const std::string &name)
    : name(name), mSourceModelSize(1.0f), mTargetModelSize(1.0f)
{

}


bool Converter::setup( ModelPtr inputModel, AbstractMotionPtr inputMotion, ModelPtr outputModel )
{
	this->inputModel = inputModel;
	this->inputMotion = inputMotion;
	this->outputModel = outputModel;
	return true;
}

bool Converter::setupXML( const std::string &xmlString )
{
	try
    {
        rapidxml::xml_document<char> doc;   // character type defaults to char
		char* y = doc.allocate_string(xmlString.c_str());
        doc.parse<0>(y);					// 0 means default parse flags

        rapidxml::xml_node<>* node = doc.first_node();
        if (!node)
        {
            MMM_ERROR << "Could not parse data in XML definition" << endl;
            return false;
        }
        return _setup(node);
    }
    catch (rapidxml::parse_error& e)
    {
        MMM_ERROR << "Could not parse data in XML definition" << endl
            << "Error message:" << e.what() << endl
            << "Position: " << endl << e.where<char>() << endl;
    }
    catch (std::exception& e)
    {
        MMM_ERROR << "Error while parsing XML definition" << endl
            << "Error code:" << e.what() << endl;
    }
    catch (...)
    {
        cout << "Error while parsing XML definition" << endl;
    }
    return false;
}

bool Converter::setupFile( const std::string &configFilename )
{
    // load file
    std::ifstream in(configFilename.c_str());

    if (!in.is_open())
    {
        MMM_ERROR << "Could not open XML file:" << configFilename << endl;
        return false;
    }

    std::stringstream buffer;
    buffer << in.rdbuf();
    std::string configXML(buffer.str());
    in.close();

    bool res = setupXML(configXML);
    if (!res)
    {
        MMM_ERROR << "Error while parsing file " << configFilename << endl;
    }

    return res;
}

void Converter::setupJointOrder( const std::vector<std::string> & jo )
{
	jointOrder = jo;
}

std::vector<std::string> Converter::getJointOrder() const
{
	return jointOrder;
}

Eigen::Matrix4f Converter::getInitialModelFitting()
{
    return initialModelPose;
}

std::map< std::string, float> Converter::getInitialJointFitting()
{
    return initialJointValues;
}


}
